/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/maps/CPointsMap.h>
#include <mrpt/obs/obs_frwds.h>
#include <mrpt/opengl/pointcloud_adapters.h>
#include <mrpt/serialization/CSerializable.h>

namespace mrpt
{
namespace maps
{
/** A map of 3D points with reflectance/intensity (float).
 * \sa mrpt::maps::CPointsMap, mrpt::maps::CMetricMap
 * \ingroup mrpt_maps_grp
 */
class CPointsMapXYZI : public CPointsMap
{
  DEFINE_SERIALIZABLE(CPointsMapXYZI, mrpt::maps)

 public:
  CPointsMapXYZI() = default;

  explicit CPointsMapXYZI(const CPointsMap& o) { CPointsMap::operator=(o); }
  CPointsMapXYZI(const CPointsMapXYZI& o) : CPointsMap() { impl_copyFrom(o); }
  CPointsMapXYZI& operator=(const CPointsMap& o)
  {
    impl_copyFrom(o);
    return *this;
  }
  CPointsMapXYZI& operator=(const CPointsMapXYZI& o)
  {
    impl_copyFrom(o);
    return *this;
  }

  /** @name Pure virtual interfaces to be implemented by any class derived
 from CPointsMap
  @{ */

  void reserve(size_t newLength) override;  // See base class docs
  void resize(size_t newLength) override;   // See base class docs
  void setSize(size_t newLength) override;  // See base class docs

  /** Get all the data fields for one point as a vector: [X Y Z I]
   *  Unlike getPointAllFields(), this method does not check for index out of
   * bounds
   * \sa getPointAllFields, setPointAllFields, setPointAllFieldsFast
   */
  void getPointAllFieldsFast(size_t index, std::vector<float>& point_data) const override
  {
    point_data.resize(4);
    point_data[0] = m_x[index];
    point_data[1] = m_y[index];
    point_data[2] = m_z[index];
    point_data[3] = m_intensity[index];
  }

  /** Set all the data fields for one point as a vector: [X Y Z I]
   *  Unlike setPointAllFields(), this method does not check for index out of
   * bounds
   * \sa setPointAllFields, getPointAllFields, getPointAllFieldsFast
   */
  void setPointAllFieldsFast(size_t index, const std::vector<float>& point_data) override
  {
    ASSERT_(point_data.size() == 4);
    m_x[index] = point_data[0];
    m_y[index] = point_data[1];
    m_z[index] = point_data[2];
    m_intensity[index] = point_data[3];
  }

  /** Loads from a Kitti dataset Velodyne scan binary file.
   * The file can be gz compressed (only enabled if the filename ends in ".gz"
   * to prevent spurious false autodetection of gzip files).
   * \return true on success */
  bool loadFromKittiVelodyneFile(const std::string& filename);

  bool saveToKittiVelodyneFile(const std::string& filename) const;

  /** See CPointsMap::loadFromRangeScan() */
  void loadFromRangeScan(
      const mrpt::obs::CObservation2DRangeScan& rangeScan,
      const std::optional<const mrpt::poses::CPose3D>& robotPose = std::nullopt) override;
  /** See CPointsMap::loadFromRangeScan() */
  void loadFromRangeScan(
      const mrpt::obs::CObservation3DRangeScan& rangeScan,
      const std::optional<const mrpt::poses::CPose3D>& robotPose = std::nullopt) override;

 protected:
  // Friend methods:
  template <class Derived>
  friend struct detail::loadFromRangeImpl;
  template <class Derived>
  friend struct detail::pointmap_traits;

 public:
  /** @} */

  /** Save to a text file. In each line contains X Y Z (meters) I (intensity)
   * Returns false if any error occurred, true elsewere.
   */
  bool saveXYZI_to_text_file(const std::string& file) const;

  /** Loads from a text file, each line having "X Y Z I", I in [0,1].
   * Returns false if any error occurred, true elsewere. */
  bool loadXYZI_from_text_file(const std::string& file);

  /** Changes a given point from map. First index is 0.
   * \exception Throws std::exception on index out of bound.
   */
  void setPointRGB(
      size_t index, float x, float y, float z, float R_intensity, float G_ignored, float B_ignored)
      override;

  /** Adds a new point given its coordinates and color (colors range is [0,1])
   */
  void insertPointRGB(
      float x, float y, float z, float R_intensity, float G_ignored, float B_ignored) override;

  /** Changes the intensity of a given point from the map. First index is 0.
   * \exception Throws std::exception on index out of bound.
   */
  void setPointIntensity(size_t index, float intensity);

  /** Like \c setPointColor but without checking for out-of-index errors */
  inline void setPointColor_fast(size_t index, float R, float G, float B)
  {
    m_intensity[index] = R;
  }

  /** Retrieves a point and its color (colors range is [0,1])
   */
  void getPointRGB(
      size_t index,
      float& x,
      float& y,
      float& z,
      float& R_intensity,
      float& G_intensity,
      float& B_intensity) const override;

  /** Retrieves a point intensity (range [0,1]) */
  float getPointIntensity(size_t index) const;

  /** Like \c getPointColor but without checking for out-of-index errors */
  inline float getPointIntensity_fast(size_t index) const { return m_intensity[index]; }

  bool hasIntensityField() const { return !m_intensity.empty(); }

  /** Returns true if the point map has a color field for each point */
  bool hasColorPoints() const override { return hasIntensityField(); }

  /** Override of the default 3D scene builder to account for the individual
   * points' color.
   */
  void getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const override;

  void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const override
  {
    std::string fil(filNamePrefix + std::string(".txt"));
    saveXYZI_to_text_file(fil);
  }

  /** @name String-keyed field access virtual interface implementation
      @{ */
  constexpr static std::string_view POINT_FIELD_INTENSITY = "intensity";

  bool hasPointField(const std::string_view& fieldName) const override;
  std::vector<std::string_view> getPointFieldNames_float() const override;

  float getPointField_float(size_t index, const std::string_view& fieldName) const override;
  void setPointField_float(size_t index, const std::string_view& fieldName, float value) override;

  void insertPointField_float(const std::string_view& fieldName, float value) override;
  void reserveField_float(const std::string_view& fieldName, size_t n) override;
  void resizeField_float(const std::string_view& fieldName, size_t n) override;

  auto getPointsBufferRef_float_field(const std::string_view& fieldName) const
      -> const mrpt::aligned_std_vector<float>* override
  {
    if (auto* f = CPointsMap::getPointsBufferRef_float_field(fieldName); f)
    {
      return f;
    }
    if (fieldName == POINT_FIELD_INTENSITY) return &m_intensity;
    return nullptr;
  }
  auto getPointsBufferRef_float_field(const std::string_view& fieldName)
      -> mrpt::aligned_std_vector<float>* override
  {
    if (auto* f = CPointsMap::getPointsBufferRef_float_field(fieldName); f)
    {
      return f;
    }
    if (fieldName == POINT_FIELD_INTENSITY) return &m_intensity;
    return nullptr;
  }
  /** @} */

  /** @name PCL library support
    @{ */

  /** Save the point cloud as a PCL PCD file, in either ASCII or binary format
   * \note This method requires user code to include PCL before MRPT headers.
   * \return false on any error */
#if defined(PCL_LINEAR_VERSION)
  inline bool savePCDFile(const std::string& filename, bool save_as_binary) const
  {
    pcl::PointCloud<pcl::PointXYZI> cloud;

    const size_t nThis = this->size();

    // Fill in the cloud data
    cloud.width = nThis;
    cloud.height = 1;
    cloud.is_dense = false;
    cloud.points.resize(cloud.width * cloud.height);

    for (size_t i = 0; i < nThis; ++i)
    {
      cloud.points[i].x = m_x[i];
      cloud.points[i].y = m_y[i];
      cloud.points[i].z = m_z[i];
      cloud.points[i].intensity = this->m_intensity[i];
    }

    return 0 == pcl::io::savePCDFile(filename, cloud, save_as_binary);
  }
#endif

  /** Loads a PCL point cloud (WITH XYZI information) into this MRPT class.
   *  Usage example:
   *  \code
   *    pcl::PointCloud<pcl::PointXYZI> cloud;
   *    mrpt::maps::CPointsMapXYZI       pc;
   *
   *    pc.setFromPCLPointCloudXYZI(cloud);
   *  \endcode
   * \sa CPointsMap::setFromPCLPointCloud()
   */
  template <class POINTCLOUD>
  void setFromPCLPointCloudXYZI(const POINTCLOUD& cloud)
  {
    const size_t N = cloud.points.size();
    clear();
    reserve(N);
    for (size_t i = 0; i < N; ++i)
    {
      const auto& pt = cloud.points[i];
      m_x.push_back(pt.x);
      m_y.push_back(pt.y);
      m_z.push_back(pt.z);
      m_intensity.push_back(pt.intensity);
    }
    mark_as_modified();
  }

  /** Like CPointsMap::getPCLPointCloud() but for PointCloud<PointXYZI> */
  template <class POINTCLOUD>
  void getPCLPointCloudXYZI(POINTCLOUD& cloud) const
  {
    const size_t nThis = this->size();
    this->getPCLPointCloud(cloud);  // 1st: xyz data
    // 2nd: I data
    for (size_t i = 0; i < nThis; ++i) cloud.points[i].intensity = m_intensity[i];
  }
  /** @} */

 protected:
  /** The intensity/reflectance data */
  mrpt::aligned_std_vector<float> m_intensity;

  /** Clear the map, erasing all the points */
  void internal_clear() override;

  /** @name Redefinition of PLY Import virtual methods from CPointsMap
    @{ */
  void PLY_import_set_vertex(
      size_t idx,
      const mrpt::math::TPoint3Df& pt,
      const mrpt::img::TColorf* pt_color = nullptr) override;

  void PLY_import_set_vertex_count(size_t N) override;

  void PLY_import_set_vertex_timestamp(
      [[maybe_unused]] size_t idx, [[maybe_unused]] const double unixTimestamp) override
  {
    // do nothing, this class ignores timestamps
  }

  /** @} */

  /** @name Redefinition of PLY Export virtual methods from CPointsMap
    @{ */
  void PLY_export_get_vertex(
      size_t idx,
      mrpt::math::TPoint3Df& pt,
      bool& pt_has_color,
      mrpt::img::TColorf& pt_color) const override;
  /** @} */

  MAP_DEFINITION_START(CPointsMapXYZI)
  mrpt::maps::CPointsMap::TInsertionOptions insertionOpts;
  mrpt::maps::CPointsMap::TLikelihoodOptions likelihoodOpts;
  MAP_DEFINITION_END(CPointsMapXYZI)

};  // End of class def.

}  // namespace maps

namespace opengl
{
/** Specialization
 * mrpt::opengl::PointCloudAdapter<mrpt::maps::CPointsMapXYZI>
 * \ingroup mrpt_adapters_grp */
template <>
class PointCloudAdapter<mrpt::maps::CPointsMapXYZI>
{
 private:
  mrpt::maps::CPointsMapXYZI& m_obj;

 public:
  /** The type of each point XYZ coordinates */
  using coords_t = float;
  /** Has any color RGB info? */
  static constexpr bool HAS_RGB = true;
  /** Has native RGB info (as floats)? */
  static constexpr bool HAS_RGBf = true;
  /** Has native RGB info (as uint8_t)? */
  static constexpr bool HAS_RGBu8 = false;

  /** Constructor (accept a const ref for convenience) */
  inline PointCloudAdapter(const mrpt::maps::CPointsMapXYZI& obj) :
      m_obj(*const_cast<mrpt::maps::CPointsMapXYZI*>(&obj))
  {
  }
  /** Get number of points */
  inline size_t size() const { return m_obj.size(); }
  /** Set number of points (to uninitialized values) */
  inline void resize(size_t N) { m_obj.resize(N); }
  /** Does nothing as of now */
  inline void setDimensions(size_t /*height*/, size_t /*width*/) {}
  /** Get XYZ coordinates of i'th point */
  template <typename T>
  inline void getPointXYZ(size_t idx, T& x, T& y, T& z) const
  {
    m_obj.getPointFast(idx, x, y, z);
  }
  /** Set XYZ coordinates of i'th point */
  inline void setPointXYZ(size_t idx, const coords_t x, const coords_t y, const coords_t z)
  {
    m_obj.setPointFast(idx, x, y, z);
  }

  /** Get XYZ_RGBf coordinates of i'th point */
  template <typename T>
  inline void getPointXYZ_RGBAf(
      size_t idx, T& x, T& y, T& z, float& r, float& g, float& b, float& a) const
  {
    m_obj.getPointRGB(idx, x, y, z, r, g, b);
    a = 1.0f;
  }
  /** Set XYZ_RGBf coordinates of i'th point */
  inline void setPointXYZ_RGBAf(
      size_t idx,
      const coords_t x,
      const coords_t y,
      const coords_t z,
      const float r,
      const float g,
      const float b,
      [[maybe_unused]] const float a)
  {
    m_obj.setPointRGB(idx, x, y, z, r, g, b);
  }

  /** Get XYZ_RGBu8 coordinates of i'th point */
  template <typename T>
  inline void getPointXYZ_RGBu8(
      size_t idx, T& x, T& y, T& z, uint8_t& r, uint8_t& g, uint8_t& b) const
  {
    float I, Gignrd, Bignrd;
    m_obj.getPoint(idx, x, y, z, I, Gignrd, Bignrd);
    r = g = b = I * 255;
  }
  /** Set XYZ_RGBu8 coordinates of i'th point */
  inline void setPointXYZ_RGBu8(
      size_t idx,
      const coords_t x,
      const coords_t y,
      const coords_t z,
      const uint8_t r,
      const uint8_t g,
      const uint8_t b)
  {
    m_obj.setPointRGB(idx, x, y, z, r / 255.f, g / 255.f, b / 255.f);
  }

  /** Get RGBf color of i'th point */
  inline void getPointRGBf(size_t idx, float& r, float& g, float& b) const
  {
    r = g = b = m_obj.getPointIntensity_fast(idx);
  }
  /** Set XYZ_RGBf coordinates of i'th point */
  inline void setPointRGBf(size_t idx, const float r, const float g, const float b)
  {
    m_obj.setPointColor_fast(idx, r, g, b);
  }

  /** Get RGBu8 color of i'th point */
  inline void getPointRGBu8(size_t idx, uint8_t& r, uint8_t& g, uint8_t& b) const
  {
    float i = m_obj.getPointIntensity_fast(idx);
    r = g = b = i * 255;
  }
  /** Set RGBu8 coordinates of i'th point */
  inline void setPointRGBu8(size_t idx, const uint8_t r, const uint8_t g, const uint8_t b)
  {
    m_obj.setPointColor_fast(idx, r / 255.f, g / 255.f, b / 255.f);
  }

};  // end of PointCloudAdapter<mrpt::maps::CPointsMapXYZI>
}  // namespace opengl
}  // namespace mrpt
